﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using KinectAdapter;
using Microsoft.Kinect;

namespace KinectAdapter
{
    public class AllInRightHand : KinectForkliftAdapter
    {
        public override event EventHandler StopIssued = null;
        public override event EventHandler MenuIssued = null;

        private const double X_DATA_SCALE = 2.0;
        private const double Y_DATA_SCALE = 1.5;
        private const double Z_DATA_SCALE = 4.0;
        private const double X_ZERO_RANGE = 0.15;
        private const double Y_ZERO_RANGE = 0.15;
        private const double Z_ZERO_RANGE = 0.15;

        /// <summary>
        /// Initiates the adapter.
        /// Start() methods needs to be called for it to start working.
        /// </summary>
        public AllInRightHand()
        {
            X = new KinectSlider() { Min = -1, Max = 1, Scale = X_DATA_SCALE, ZeroRange = X_ZERO_RANGE};
            Y = new KinectSlider() { Min = -1, Max = 1, Scale = Y_DATA_SCALE, ZeroRange = Y_ZERO_RANGE };
            Z = new KinectSlider() { Min = -1, Max = 1, Scale = Z_DATA_SCALE, ZeroRange = Z_ZERO_RANGE };
        }

        /// <summary>
        /// Handles the skeleton data and changes the values of control.
        /// </summary>
        /// <param name="skeleton">Skeleton data received.</param>
        protected override void GetSkeletonData(Skeleton skeleton)
        {
            MainSteeringOn = CheckMainSteering(skeleton);
            SecondarySteeringOn = CheckSecondarySteering(skeleton);

            if (!firstFrame)
            {
                if (MainSteeringOn)
                {
                    double xChange = skeleton.Joints[JointType.HandRight].Position.X - handRightLastX;
                    double zChange = handRightLastZ - skeleton.Joints[JointType.HandRight].Position.Z;  // reversed axis
                    X.ChangeValue(xChange);
                    Z.ChangeValue(zChange);
                }
                if (SecondarySteeringOn)
                {
                    double yChange = skeleton.Joints[JointType.HandRight].Position.Y - handRightLastY;
                    Y.ChangeValue(yChange);
                }
            }

            if (CheckStop(skeleton))
            {
                if (CheckMenu(skeleton))    // menu gesture contains the stop gesture
                {
                    if (MenuIssued != null)
                    {
                        MenuIssued(this, null);
                    }
                    return;
                }
                else  // not menu, just stop
                {
                    if (StopIssued != null)
                    {
                        StopIssued(this, null);
                    }
                }
            }

            UpdateSteeringHandPositions(skeleton);
            firstFrame = false;
        }


        /// <summary>
        /// Check if the main steering should be enabled.
        /// It will be when right hand is below the right hip.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the main steering should be enabled.</returns>
        protected override bool CheckMainSteering(Skeleton skeleton)
        {
            return skeleton.Joints[JointType.HandLeft].Position.Y <= skeleton.Joints[JointType.HipLeft].Position.Y;
        }

        /// <summary>
        /// Check if the secondary steering should be enabled.
        /// It will be when right hand is below the right hip.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the secondary steering should be enabled.</returns>
        protected override bool CheckSecondarySteering(Skeleton skeleton)
        {
            return skeleton.Joints[JointType.HandLeft].Position.Y <= skeleton.Joints[JointType.HipLeft].Position.Y;
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that all controls
        /// should be restored to their defaults (stop the forklift).
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the forklift should stop.</returns>
        protected override bool CheckStop(Skeleton skeleton)
        {
            SkeletonPoint left = skeleton.Joints[JointType.HandLeft].Position;
            SkeletonPoint right = skeleton.Joints[JointType.HandRight].Position;

            // both hands above shoulders
            if (left.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y
                && right.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y)
            {
                // hands touching
                if (OperatorSkeleton.GetDistance(left, right) <= JOINT_TOUCH_RANGE)
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that application
        /// should return to menu. HUD will still be working but will be hidden.
        /// Also, the forklift should stop.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the application should return to menu.</returns>
        protected override bool CheckMenu(Skeleton skeleton)
        {
            // first, the condition from stop needs to be met
            if (CheckStop(skeleton))
            {
                double dist = OperatorSkeleton.GetDistance(skeleton.Joints[JointType.ElbowLeft].Position,
                    skeleton.Joints[JointType.ElbowRight].Position);

                // if the elbows are touching
                if (dist <= JOINT_TOUCH_RANGE)
                    return true;
            }
            return false;
        }
    }
}
